﻿import sys
import signal
import Hobot.GPIO as GPIO
import time
from track_fsm import TrackFsm;


Light_In = 16

# 输出引脚
LeftWheel_F = 33
LeftWheel_B = 37
RightWheel_F = 32
RightWheel_B = 36

pwm_l = None
pwm_r = None


trackFsm = TrackFsm()
trackFsm.set_state("None")
trackFsm.backup_mode = False



def setup():
    GPIO.cleanup()
    GPIO.setmode(GPIO.BOARD)

    GPIO.setup(Light_In, GPIO.IN,initial=GPIO.HIGH)
    


    global pwm_l, pwm_r
    pwm_l = GPIO.PWM(LeftWheel_F,48000)
    pwm_r = GPIO.PWM(RightWheel_F,48000)

    GPIO.setup(LeftWheel_B, GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(RightWheel_B, GPIO.OUT,initial=GPIO.LOW)

    pwm_l.ChangeDutyCycle(1)#pwm能用
    pwm_r.ChangeDutyCycle(1)#pwm无法变更值

    pwm_l.start(0)
    pwm_r.start(0)

    print("设置完毕")


def loop():

    while True:
        light = GPIO.input(Light_In)#高电平 无感知 低电平 感知到光
        print("光感知结果{0}".format(light))
        if not light: 
            trackFsm.set_state("Mid")
        else:
            trackFsm.set_state("None")

        # 驱动运动
        trackFsm.apply_motor_drive()
        time.sleep(0.1) #间隔问题




def destroy():

    GPIO.setup(LeftWheel_B, GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(RightWheel_B, GPIO.OUT,initial=GPIO.LOW)

    pwm_l.stop()
    pwm_r.stop()

    GPIO.cleanup()


def set_GPIO_value(lf,lb,rf,rb):

    # print("{0} {1} {2} {3}".format(lf,lb,rf,rb))


    pwm_l.ChangeDutyCycle(lf)
    pwm_r.ChangeDutyCycle(rf)
    GPIO.output(LeftWheel_B,lb)
    GPIO.output(RightWheel_B,rb)




def fsm_init():

    # 定义callback
    def none_state_callback():
        # print("执行了none回调")
        set_GPIO_value(0,0,0,0)

    def left_state_callback():
        # print("执行了turnleft回调")
        set_GPIO_value(0,1,90,0)

    def right_state_callback():
        # print("执行了turnRight回调")
        set_GPIO_value(65,0,0,0)

    def min_left_state_callback():
        # print("执行了minleft回调")
        set_GPIO_value(65,0,75,0)

    def min_right_state_callback():
        # print("执行了minright回调")
        set_GPIO_value(75,0,65,0)

    def mid_state_callback():
        # print("执行了mid回调")
        set_GPIO_value(70,0,70,0)


    trackFsm.set_state_drive_callback("None",none_state_callback)
    trackFsm.set_state_drive_callback("TurnLeft",left_state_callback)
    trackFsm.set_state_drive_callback("TurnRight",right_state_callback)
    trackFsm.set_state_drive_callback("TrendLeft",min_left_state_callback)
    trackFsm.set_state_drive_callback("TrendRight",min_right_state_callback)
    trackFsm.set_state_drive_callback("Mid",mid_state_callback)






if __name__ == '__main__':
    setup()
    fsm_init()
    try:
        loop()
    except KeyboardInterrupt:
        destroy()
    finally:
        destroy()